/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import file.Configurable;
import file.Configuration;

/**
 *
 * @author Mentors
 */
public class Camera implements Configurable{

    // Servo Rates - controls how fast camera is moved in response to joystick input
    private static final double kCameraLeftRightRate = .02;
    private static final double kCameraUpDownRate    = .02;

    private static int servoMax = 245;
    private static int servoMin = 11;

    // Sensors

    AxisCamera cam;
    
    // Actuators

    private static Servo      cameraLeftRightServo; // moves camera left and right
    private static Servo      cameraUpDownServo;    // moves camera up and down

    private static double lrServoSet = .5;          // controls left/right camera movement
    private static double udServoSet =  0;          // controls up/down camera movement
    
    private static MessageCenter messageCenter;

    private static Camera m_instance = null;

    /**
     *  Get an instance of the Camera
     */
    public static Camera getInstance(Servo leftRightServo, Servo upDownServo) {

        if (m_instance == null) {
            m_instance = new Camera();

        }

        // create the digital output objects to write to the shape indicator LEDs

        cameraLeftRightServo = leftRightServo;
        cameraUpDownServo    = upDownServo;
        cameraLeftRightServo.setBounds(servoMax, 0, 0, 0, servoMin);
        
        Configuration.getInstance().register("Camera", m_instance);

        return m_instance;
    }

    public static Camera getInstance() {
        if (m_instance == null) {
            m_instance = new Camera();
        }
        return m_instance;
    }


    private Camera () {

        cam = AxisCamera.getInstance();
        cam.writeResolution(AxisCamera.ResolutionT.k320x240);
        cam.writeBrightness(0);
        cam.writeMaxFPS(10);
        cam.writeRotation(AxisCamera.RotationT.k180);

        messageCenter = MessageCenter.getInstance();
    }


    /**
     * move is called to move the camera using inputs from the
     * operator joystick
     */
    public void move(double operatorStickX, double operatorStickY) {

        // only move the camera if the joystick is near the extremes
        if (operatorStickX > 0.8)
        {
            lrServoSet += kCameraLeftRightRate;
            if (lrServoSet > 1)
            {
                lrServoSet = 1.0;
            }
            cameraLeftRightServo.set(lrServoSet);
        }
        if (operatorStickX < -0.8)
        {
            lrServoSet -= kCameraLeftRightRate;
            if (lrServoSet < 0)
            {
                lrServoSet = 0;
            }
            cameraLeftRightServo.set(lrServoSet);
        }

        // for 2011, the camera only pans; it does NOT tilt.  The up/down servo
        // is NOT instaniated, so it must not be referenced!!
//        if (operatorStickY > 0.8)
//        {
//            udServoSet += kCameraUpDownRate;
//            if (udServoSet > 1)
//            {
//                udServoSet = 1.0;
//            }
//            cameraUpDownServo.set(udServoSet);
//        }
//        if (operatorStickY < -0.8)
//        {
//            udServoSet -= kCameraUpDownRate;
//            if (udServoSet < 0)
//            {
//                udServoSet = 0;
//            }
//            cameraUpDownServo.set(udServoSet);
//        }

        messageCenter.println(MessageCenter.kActuatorsScreen, MessageCenter.kLine1,
              "Camera LR Set: " + messageCenter.formatDouble(lrServoSet, 6));
        messageCenter.println(MessageCenter.kActuatorsScreen, MessageCenter.kLine2,
              "Camera UD Set: " + messageCenter.formatDouble(udServoSet, 6));
    }

    public void update(String name, String[] values) {

        if (name.equals("Servo.Max")) {
            servoMax = Integer.parseInt(values[0]);
            cameraLeftRightServo.setBounds(servoMax, 0, 0, 0, servoMin);

        } else if (name.equals("Servo.Min")) {
            servoMin = Integer.parseInt(values[0]);
            cameraLeftRightServo.setBounds(servoMax, 0, 0, 0, servoMin);

        }
    }

}
